#include "Random.hpp"
#include "simulate/birtnorm.hpp"
#include "simulate/rtnomNC.hpp"
#include "simulate/rtnorm.hpp"
#include <boost/timer.hpp>
#include "simulate/src/rtnormNC.hpp"
#include "Data/Data.hpp" 

#define ENABLE 1 
//#define DEPTH 5 

template<class Resample>
class GhkBS
{
	public:
	GhkBS(Resample *R){

		boost::random::uniform_01<> U;
		_runif=new RandomG::Random<boost::random::uniform_01<> >(U);
		_R=R;
		d=new boost::math::normal_distribution<>(0,1); 
		rt=new rtnormNC();
		rt2=new rtnorm();
		boost::random::normal_distribution<> N2;
		_rnorm= new RandomG::Random<boost::random::normal_distribution<> >(N2);
		sigma=0.1;
	 	int _nu=5;	
		boost::random::gamma_distribution<> N3(_nu/2,2);
		rgamma= new RandomG::Random<boost::random::gamma_distribution<> >(N3);
		Birt=new Birtnorm();
		rt3=new rtNC();
	}
	~GhkBS()
	{
		delete d;
		delete _runif;
	}

	void ghk_oneside(double *L, double* trunpt, int *above, int *dim,double *eta, int *n, int *depth, double *res)
	/*	routine to implement ghk with a region defined by truncation only on one- side
							r mcculloch 8/04
		if above=1, then we truncate component i from above at point trunpt[i-1]
		L is lower triangular root of Sigma
		random vector is assumed to have zero mean
		n is number of draws to use in GHK	
		modified 6/05 by rossi to check arg into qnorm
		modified 2012 by ridgway to make it awesome!!!! 
	 */
	{
	   int i,j;
	   double mu,tpz,u,pa,pb,arg;
	   int *dim2=new int[1];
	   dim2[0]=2;
	   mat z(*n,*dim);
	   double *prod=new double[*n];
	   Normalize(prod,n);
	   double thres=*eta*(*n);
	   z.fill(0);

	   int *above2=new int[2];
	   above2[0]=0;
	   above2[1]=0;

	  // sobol::Owenscramble O;
	   //O(*n,*dim,0,Nt);
	   *res = 0;
	   int kk=0;
	   int b=0;
	 // mat X=vect2mat<double>(L,*dim,*dim);
	 //cout << X;
	 //mat U(1,*dim);
	 //U.fill(0);
	   mat a(2,1);
	//SampleGibbs(dim,above,trunpt,L,&U); 
	      for(j=0;j<*dim;j++) {
			//Resample
		      double ess=Ess(prod,n);
		      cout << "ess " << ess;
		      if(ess<thres){	
				*res+=mean2(prod,n);
		      		fResample(prod,n,&z);
				cout << "How di"; 
	   		}
		      if(j>=1)
		      {
		      
							// set weigths	      
			for(i=0;i<*n;i++) {
					
					//cout << " " << prod[i];
									//	cout << a.t();
				if(ENABLE==1)
				{
					mat Gamma(2,2);
					Gamma(0,0)=L[(j-1)*(*dim)+j-1];
					Gamma(1,0)=L[(j-1)*(*dim)+j];
					Gamma(1,1)=L[j*(*dim)+j];
					Gamma(0,1)=0;
					mat Sig=Gamma*Gamma.t();
				/*	double rho=Sig(0,1)/(sqrt(Sig(0,0)*Sig(1,1)));
					double nu=sqrt(1-rho*rho);
					double a_1=b1/sqrt(Sig(0,0));
					double a_2=b2/sqrt(Sig(1,1));*/
					double b1=trunpt[j-1];			
					double b2=trunpt[j];			
					mat b(2,1);
					b(0,0)=b1;
					b(1,0)=b2;
					double b3=b2;
					for(int k=0;k<j-1;k++)
					{
						b1-=L[k*(*dim)+(j-1)]*z(i,k);	
						b2-=L[k*(*dim)+j]*z(i,k);	
					}
					mat x(2,1);
				
				      if(Gamma(1,0)>0)
				      {

						mu=0.0; for(int k=0;k<j;k++) mu += L[k*(*dim)+j]*z(i,k);
						 tpz = (trunpt[j]-mu)/L[j*(*dim)+j];
						 if(above[j]) {
						    pa=0.0; pb = cdf(*d,tpz);   
						 }
						 else {
						    pb=1.0; pa = cdf(*d,tpz);
						 }
							 prod[i]*=pb-pa;
							//cout << prod[i] << " "; 
						u = (*_runif)();
						 arg=u*pb+(1.-u)*pa;
						 if(arg > .999999999) arg=.999999999;
						 if(arg < .0000000001) arg=.0000000001;
						 z(i,j) = quantile(*d,arg);	

									
					}else{
						x(1,0)=(*rt)(0,(b2-Gamma(1,0)*z(i,j-1))/Gamma(1,1),pINF,1);								 		
						x(0,0)=(*rt)(0,b1/Gamma(0,0),(b2-Gamma(1,1)*x(1,0))/Gamma(1,0),1);
						pa=cdf(*d,(b2-Gamma(1,0)*z(i,j-1))/Gamma(1,1));	
						pb=1;
						prod[i]*=pb-pa;
						z(i,j-1)=x(0,0);
						z(i,j)=x(1,0);
					
					
					 /*
/////////////////////////////////////////////////////////////////////
						x=(*Birt)(Sig,b);	
						pa=cdf(*d,b2/(Gamma(1,0)*sqrt(Gamma(1,1)*Gamma(1,1)/(Gamma(1,0)*Gamma(1,0))+1)))-cdf(*d,b1/Gamma(0,0));
						prod[i]*=1-pa;
						z(i,j-1)=x(0,0);
						z(i,j)=x(1,0);
						*/
					
					}
				}else{
				        mu=0.0; for(int k=0;k<j;k++) mu += L[k*(*dim)+j]*z(i,k);
					tpz = (trunpt[j]-mu)/L[j*(*dim)+j];
					if(above[j]){
					    pa=0.0; pb = cdf(*d,tpz);   
					}
					else {
					    pb=1.0; pa = cdf(*d,tpz);
					 }
					 prod[i]*=pb-pa;
						//cout << prod[i] << " "; 
				         u = (*_runif)();
					 arg=u*pb+(1.-u)*pa;
					 if(arg > .999999999) arg=.999999999;
					 if(arg < .0000000001) arg=.0000000001;
					 z(i,j) = quantile(*d,arg);	


				/*	z(i,j)=(*rt)(0,(b2-Gamma(1,0)*z(i,j-1))/Gamma(1,1),pINF,1);
				pa=cdf(*d,(b2-Gamma(1,0)*z(i,j-1))/Gamma(1,1));	
				prod[i]*=1-pa;*/
				//	if(L[(j-1)*(*dim)+j]<0)
				//	{
						int nb_Gibbs=max(j-*depth,0);
						GibbsMove(dim,n,above,trunpt,L,&z,j,nb_Gibbs);
				//	}
				}

		      }
		//		cout << "\n\\\\\\\\\\\\\\\\\\\\\\\n";
		      }else{
			for(i=0;i<*n;i++) {
				 
				mu=0.0; for(int k=0;k<j;k++) mu += L[k*(*dim)+j]*z(i,k);
				 tpz = (trunpt[j]-mu)/L[j*(*dim)+j];
				 if(above[j]) {
				    pa=0.0; pb = cdf(*d,tpz);   
				 }
				 else {
				    pb=1.0; pa = cdf(*d,tpz);
				 }
					 prod[i]*=pb-pa;
					//cout << prod[i] << " "; 
				u = (*_runif)();
				 arg=u*pb+(1.-u)*pa;
				 if(arg > .999999999) arg=.999999999;
				 if(arg < .0000000001) arg=.0000000001;
				 z(i,j) = quantile(*d,arg);
      			}
	
		      }
	      }
	 *res+=mean2(prod,n);
	 
	 cout << "resBS " << *res;
	 delete[] prod;
	 delete[] above2;
	 delete[] dim2;
	}

	inline void Normalize(double *w,int *n)
	{
		for(int i=0;i<*n;i++)
		{
			w[i]=1;
		}
	}
	inline double mean2(double *w, int *n){
		double sum=log(w[0]);
		for(int i=1;i<*n;i++){
			if(w[i]!=0)
			{
				sum=log_add(log(w[i]),sum);
			}
		}
		return sum-log(*n);
	}
		
	inline void fResample(double *w, int *n,mat *z)
	{
		mat v=growingvect(*n);
		(*_R)(n,w,v);
		//cout << v.t();
		Arangemat(*z,v);
		Normalize(w,n);
	}
	inline double Ess(double *w, int *n)
	{
		double sum=0;
		for(int i=0;i<*n;i++)
		{
			sum+=w[i];
		}
		double sum2=0;
		for(int i=0;i<*n;i++)
		{
			//cout << w[i]/sum << " ";
			sum2+=pow(w[i]/sum,2);
		}
		return (double)1/sum2;

	}
	double propSp(double x1,double x2,mat Sig,double rho, double nu, double b1, double b2)
	{
		double t1=dnormp(x1,0,sqrt(Sig(0,0)))*dnormp(x2,rho*x1*sqrt(Sig(1,1)),sqrt(Sig(1,1))*nu);
		if(t1==0) t1=0.0000000001;
		double t2=(1-cdf(*d,b1/sqrt(Sig(0,0))))*(1-cdf(*d,(b2-rho*x1*sqrt(Sig(1,1)))/(sqrt(Sig(0,0))*nu)));
		if(t2==0) t2=0.0000000001;
		return t1/t2;
	}
	double propSm(double x1,double x2,mat Sig, double rho, double nu, double b1, double b2)
	{

		double t1=dnormp(x1,rho*b2,sqrt(Sig(0,0))*nu)*dnormp(x2,rho*x1*sqrt(Sig(1,1)),sqrt(Sig(1,1))*nu);
		if(t1==0) t1=0.0000000001;
		double t2=(1-cdf(*d,(b1-rho*b2)/(sqrt(Sig(0,0))*nu)))*(1-cdf(*d,(b2-rho*x1*sqrt(Sig(1,1)))/(sqrt(Sig(0,0))*nu)));
		if(t2==0){
		       	t2=0.0000000001;
		}
		return t1/t2;
	}
	double mpropSp(double x1,mat Sig,double rho, double nu, double b1, double b2)
	{
		double t1=dnormp(x1,0,sqrt(Sig(0,0)));
		if(t1==0) t1=0.0000000001;
		double t2=(1-cdf(*d,b1/sqrt(Sig(0,0))));
		if(t2==0) t2=0.0000000001;
		return t1/t2;
	}
	double mpropSm(double x1,mat Sig, double rho, double nu, double b1, double b2)
	{

		double t1=dnormp(x1,rho*b2,sqrt(Sig(0,0))*nu);
		if(t1==0) t1=0.0000000001;
		double t2=(1-cdf(*d,(b1-rho*b2)/(sqrt(Sig(0,0))*nu)));
		if(t2==0){
		       	t2=0.0000000001;
		}
		return t1/t2;

	}
	double dnormp(double x, double m, double s)
	{
		return (double)1/(sqrt(2*PI)*s)*exp(-0.5*(x-m)*(x-m)/(s*s));
	}
	void GibbsMove(int *dim,int *n,int *above,double *trunpt,double *L, mat *z, int j, int depth)
	{
		int k;
		double mu,tpz,u,pa,pb,arg;
		double ltrunc=-numeric_limits<double>::infinity();
		double rtrunc=+numeric_limits<double>::infinity();
		double curr=0;
		mat Z(*n,*dim);
		Z=*z;
		int b1=1;
		int b2=1;
		for(int i=0;i<*n;i++)
		{
			for(int l=depth;l<j;l++)
			{
				b1=1;
				b2=1;
				//calcul du max
				//cout << "new " << "\n";
					ltrunc=-numeric_limits<double>::infinity();
					rtrunc=numeric_limits<double>::infinity();
					for(int ii=l;ii<j;ii++)
					{

						if(L[l*(*dim)+ii]!=0)
						{
							double temp=trunpt[ii];
							//cout << " trunc " << trunpt[ii] << " ";
							for(int jj=0;jj<=ii;jj++)
							{
								if(jj!=l)
								{
									temp-=L[jj*(*dim)+ii]*(Z)(i,jj);
						//			cout << (*z)(i,jj) << " ";
								}
							}
						//	cout << "\n";
							//cout <<  "temp1 "<<temp << " ";
							temp=temp/L[l*(*dim)+ii];
							//cout <<  "temp2 "<<temp << " ";
							if(L[l*(*dim)+ii]<0)
							{
							//	cout << "r" ;
								//cout << "rtrunc noc "<<  temp << "\n";
								if(rtrunc>temp)
								{
									b1=0;
									rtrunc=temp;
									//cout << "rtrunc "<<  temp << "\n";
								}
							}else{
							//	cout << "l" ;
								//cout << "ltrunc noc "<<  temp << "\n";
								if(ltrunc<temp)
								{
									b2=0;
									ltrunc=temp;
									//cout << "ltrunc "<<  temp << "\n";
								}
							}
						//cout <<" div " << L[l*(*dim)+ii] << " " ;
					
						}
					}
			//	cout << "l "<<ltrunc;
			//	cout << " r "<< rtrunc<< "\n";
			//	double me=as_scalar(sum((*z)(i,span::all))-(*z)(i,l));
				if(b1)
				{
					Z(i,l)=(*rt)(0,ltrunc,pINF,1);
				}else if(b2){
					Z(i,l)=(*rt)(0,-pINF,rtrunc,1);
				}else{
					//Z(i,l)=(*rt)(0,ltrunc,rtrunc,1);
				//	cout << " l "<< ltrunc << " r " << rtrunc << " ";
					if(ltrunc<rtrunc)
					{
						Z(i,l)=(*rt3)(ltrunc,rtrunc,0,1);
					}else{

					//	cout << "oups!!"; 
					}
				//	cout << "z " << Z(i,l);
				}
				//cout << "z " << Z(i,l);
			//	cout << "\n";
			}
			//				cout << "j "<<j<< " ";
							//cout << *z;
		}
		*z=Z;
	}




private:
	RandomG::Random<boost::random::uniform_01<> > *_runif;
	rtnormNC *rt;
	rtnorm *rt2;
	Resample *_R;
	boost::math::normal_distribution<> *d;
	RandomG::Random<boost::random::normal_distribution<> > *_rnorm;
	RandomG::Random<boost::random::gamma_distribution<> > *rgamma;
	double sigma;
	Birtnorm* Birt;
	rtNC *rt3;
};

